import numpy as np
import cv2
import math

def calc_angle(x):
    # 判断斜率为正或负
    if x >= 0:
        angle = np.rad2deg(np.arctan(x))
    else:
        angle = 180 + np.rad2deg(np.arctan(x))
    return angle

def calc_angle_unsign(x):
    a = math.atan(x)  # 10为斜率
    return math.degrees(a)

def line_from_two_points(point1, point2):
    if point2[0] - point1[0] == 0:
        return 0,0
    # 计算斜率
    m = (point2[1] - point1[1]) / (point2[0] - point1[0])

    # 计算截距
    b = point1[1] - (m * point1[0])

    return m, b

def recal(arr):
    min_y = 10000
    max_y = 0
    for row in arr:
        if row[1] > max_y:
            max_y = row[1]
        if row[1] < min_y:
            min_y = row[1]
    return (min_y,max_y)

def recalMaxPorint(arr):
    max_x = 0
    max_point = {}
    for row in arr:
        if row[0] > max_x:
            max_point = {"x": row[0], "y": row[1]}
            max_x = row[0]
    return max_point

def recalMinPorint(arr):
    min_x = 10000
    min_point = {}
    for row in arr:
        if row[0] < min_x:
            min_point = {"x": row[0], "y": row[1]}
            min_x = row[0]
    return min_point

def get_rate(imagename,font,lower_red,higher_red):
    frame = cv2.imread(imagename)
    height = frame.shape[0]
    print(height)
    img_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask_red = cv2.inRange(img_hsv, lower_red, higher_red)  # 可以认为是过滤出红色部分，获得红色的掩膜
    mask_red = cv2.medianBlur(mask_red, 1)  # 中值滤波
    cnts1, hierarchy1 = cv2.findContours(mask_red, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # 轮廓检测

    alignarr = []
    max_point = {}
    max_x = 0
    min_y = 10000
    max_y = 0
    for cnt in cnts1:
        (x, y, w, h) = cv2.boundingRect(cnt)  # 该函数返回矩阵四个点
        cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255), 2)  # 将检测到的颜色框起来
        if x > max_x:
            max_point = {"x":x + w / 2,"y": y + h / 2}
            max_x = x
        if y < min_y:
            min_y = y
        if y > max_y:
            max_y = y
        alignarr.append((x + w / 2, y + h / 2))
        cv2.putText(frame, 'red', (x, y - 5), font, 0.7, (0, 0, 255), 2)

    print(alignarr)
    size = len(alignarr)
    x1, y1 = alignarr[0]
    x2, y2 = alignarr[size - 1]

    bottom_cnt = 0
    for point_option in alignarr:
        if height - point_option[1] < 300 and (y2 - y1) < height / 2:
            bottom_cnt += 1
    del alignarr[:bottom_cnt]
    print(alignarr)
    size = len(alignarr)
    x2, y2 = alignarr[size - 1]
    min_y,max_y = recal(alignarr)
    max_point = recalMaxPorint(alignarr)
    print(max_point)
    cv2.putText(frame, 'max', (int(max_point["x"]),int(max_point["y"])), font, 0.7, (0, 255, 255), 2)
    min_point = recalMinPorint(alignarr)
    cv2.putText(frame, 'min', (int(min_point["x"]),int(min_point["y"])), font, 0.7, (0, 255, 255), 2)

    #cv2.imshow('target', frame)
    #cv2.waitKey(0)

    if x2 == x1:
        return 0
    rate = (y2 - y1) / (x2 - x1)
    angle_deg = calc_angle(rate)
    angle_deg_unsign = calc_angle_unsign(rate)
    print(angle_deg)
    print(min_y)


    if min_y < 100 and (y2-y1) < height/2:
        print("离顶部太近，轨迹线太短")
        #cv2.imshow('target', frame)
        #cv2.waitKey(0)
        return 0

    print(max_y)
    print(height)
    if height-max_y < 400 and (y2-y1) < height/2:
        print("离底部太近，轨迹线太短")
        print(bottom_cnt)
        #cv2.imshow('target', frame)
        #cv2.waitKey(0)
        return 0

    """
    a = math.atan(rate)  # 10为斜率
    angle_deg = math.degrees(a)
    """
    print(max_point)
    print(min_point)
    if int(max_point["x"]) > x1 and int(max_point["x"]) > x2:
        left_rate = (max_point["y"] - y1) / (max_point["x"] - x1)
        left_angle = calc_angle(left_rate)
        print(left_angle)
        print(angle_deg)
        print(math.fabs(left_angle - angle_deg))
        if math.fabs(left_angle - angle_deg) > 30:
            cv2.imshow('target', frame)
            cv2.waitKey(0)
            print('偏离角度太大')
            return 0
    elif int(min_point["x"]) < x1 and int(min_point["x"]) < x2:
        left_rate = (min_point["y"] - y1) / (min_point["x"] - x1)
        left_angle = calc_angle(left_rate)
        print(left_angle)
        print(angle_deg)
        print(math.fabs(left_angle - angle_deg))
        if math.fabs(left_angle - angle_deg) > 30:
            cv2.imshow('target', frame)
            cv2.waitKey(0)
            print('偏离角度太大')
            return 0

    #print(angle_deg)
    #cv2.imshow('target', frame)
    #print(alignarr)
    #key = cv2.waitKey(0)
    print("------------图片正常----------------------")
    return angle_deg

if __name__ == "__main__":
    font = cv2.FONT_HERSHEY_SIMPLEX
    lower_red = np.array([0, 127, 128])  # 红色阈值下界
    higher_red = np.array([10, 255, 255])  # 红色阈值上界

    for i in range(1,28):
        if i == 4:
            continue
        print("======================images/"+str(i)+".jpg===========================")
        print(get_rate("images/"+str(i)+".jpg",font,lower_red,higher_red))
